#include "PosResolve.h"
#include "ManifoldSerial.h"

//0507
#include "dji_control.hpp"
#include "dji_vehicle.hpp"
#include "flight_control_sample.hpp"

//0503
//#include <fstream>

//target_track
#include "camera_gimbal_sample.hpp"
using namespace DJI;
using namespace DJI::OSDK;
using namespace DJI::OSDK::Telemetry;
//target_track

extern "C"
{
    int manifold_cam_init(int mode);
    int manifold_cam_read(unsigned char *buffer, unsigned int *nframe, unsigned int block);
    int manifold_cam_exit();
}
void push_feature_points_position();
void far_resolve(Mat frame);
Telemetry::Vector3f toEulerAngle(void* quaternionData);
extern CamPos DJI_Z3_Pos;
RotationAngle initialAngle;
RotationAngle currentAngle;

extern Vehicle*   vehicle;
extern int G_mission_statues;
extern int fd;

//0510
extern TargetRect DJI_TargetRect;
//0503
//ofstream fout_gimbal("para_gimbal.txt");
//ofstream fout_resolve("para_resolve.txt");

//0504
ofstream fout_Quad("para_quad.txt");
ofstream fout_Euler("para_euler.txt");
ofstream fout_Yaw_Tar2Body("yaw.txt");

//0508
static int CMD_START_LANDING = 0;
static int LANDING_STATE = 0;   //0:SEARCH,1:APPROACH,2:BEGIN LANDING,3:END OF LANDING

static unsigned char my_buffer[FRAME_SIZE+8] = {0};
static unsigned int nframe = 0;
static int mode = 6;
static void *get_images_loop(void *data)
{
    int ret;

    while(!manifold_cam_exit()) /*Ctrl+c to break out*/
    {
        if(mode & GETBUFFER_MODE)
        {
#if BLOCK_MODE
            ret = manifold_cam_read(my_buffer, &nframe, CAM_BLOCK); /*blocking read*/
            if(ret < 0)
            {
                printf("manifold_cam_read error \n");
                break;
            }

#else
            ret = manifold_cam_read(my_buffer, &nframe, CAM_NON_BLOCK); /*non_blocking read*/
            if(ret < 0)
            {
                printf("manifold_cam_read error \n");
                break;
            }
#endif
        }

        usleep(1000);
    }

    printf("get_images_loop thread exit! \n");
}


int main(int argc, char** argv )
{
    DJI::OSDK::Gimbal::AngleData gimbalAngle;

    int ret;
    if(0 != geteuid())
    {
        printf("Please run ./test as root!\n");
        //print_usage(argv[0]);
        return -1;
    }
    ret = manifold_cam_init(mode);
    if(-1 == ret)
    {
        printf("manifold init error \n");
        return -1;
    }
    pthread_attr_t attr;
    struct sched_param schedparam;
    pthread_t read_thread;
    printf("main running");
    pthread_attr_init(&attr);
    pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
    pthread_attr_setschedpolicy((pthread_attr_t *)&attr, SCHED_FIFO);
    schedparam.sched_priority = 90;
    pthread_attr_setschedparam(&attr,&schedparam);
    pthread_attr_setscope(&attr, PTHREAD_SCOPE_SYSTEM);
    if (pthread_create(&read_thread, &attr, get_images_loop, NULL) != 0)
    {
        perror ("usbRead_thread create");
        assert(0);
    }
    if(pthread_attr_destroy(&attr) != 0)
    {
        perror("pthread_attr_destroy error");
    }


    push_feature_points_position();
    int frame_tag = 0;

    //
    // 1:Setup the OSDK: Read config file, create vehicle, activate.
    LinuxSetup linuxEnvironment(argc, argv);
    vehicle = linuxEnvironment.getVehicle();
    if (vehicle == NULL)
    {
      std::cout << "Vehicle not initialized, exiting.\n";
      return -1;
    }

    //gimbal thread
    pthread_t gimbal_thread;
    if (pthread_create(&gimbal_thread, NULL, gimbalCameraControl, NULL) != 0)
    {
        perror ("GimbalControl_thread create");
        assert(0);
    }

    //Manifold Serial
    fd = ManifoldSerialInitial();

    // 2:Obtain Control Authority
    int functionTimeout = 1;
    vehicle->obtainCtrlAuthority(functionTimeout);
//    monitoredTakeoff(vehicle);
//    moveByPositionOffset(vehicle, 0, 6, 6, 30);
//    moveByPositionOffset(vehicle, 6, 0, -3, -30);
//    moveByPositionOffset(vehicle, -6, -6, 0, 0);
//    monitoredLanding(vehicle);

    while(1)
    {
        //withthread
        Mat frame;
        Mat yuvImg;
        yuvImg.create(720*3/2,1280,CV_8UC1);
        memcpy(yuvImg.data, my_buffer, 1280*720*3/2*sizeof(unsigned char));
        cvtColor(yuvImg, frame, CV_YUV2BGR_NV12);
        if(frame.empty()) break;

        double time;
        time= getTickCount();

        cout<<"DJI_TargetRect.TargetMode: "<<DJI_TargetRect.TargetMode<<std::endl;
        switch(DJI_TargetRect.TargetMode)
        {
            case 0:far_resolve(frame);
                break;
            case 1:near_resolve(frame);
                break;
            default:break;
        }



        waitKey(1);

        time = 1000*((double)getTickCount() - time)/getTickFrequency();
        cout<<time<<"ms"<<endl;
        frame_tag++;
        //cout<<"frame_tag:"<<frame_tag<<endl;

        //test Quad2Euler
        Telemetry::Quaternion broadcastQ;
        float yawInDeg;
        float pitchInDeg;
        float rollInDeg;
        broadcastQ = vehicle->broadcast->getQuaternion();
        Telemetry::Vector3f EulerAngle_M100;
        EulerAngle_M100 = toEulerAngle((static_cast<void*>(&broadcastQ)));
        yawInDeg = EulerAngle_M100.z*180.0/3.14159;
        pitchInDeg = EulerAngle_M100.x*180.0/3.14159;
        rollInDeg = EulerAngle_M100.y*180.0/3.14159;
        currentAngle.roll  = vehicle->broadcast->getGimbal().roll;
        currentAngle.pitch = vehicle->broadcast->getGimbal().pitch;
        currentAngle.yaw   = vehicle->broadcast->getGimbal().yaw;
        //cout<<"delta_yaw:"<<float(currentAngle.yaw - yawInDeg) <<std::endl;
        float Yaw_Target2Body;
        Yaw_Target2Body = DJI_Z3_Pos.yaw*180.0/3.141593 - currentAngle.yaw + yawInDeg;

        //serial read
        int nread;
        char buff[30];
        if((nread=read(fd,buff,30))>0)
        {
            buff[nread]='\0';
            //printf("received data:%s\t CMD:%d\n",buff,CMD_START_LANDING);
            if(strcmp(buff,"beginlanding")==0)
            {
                CMD_START_LANDING = 1;
            }
            if(strcmp(buff,"stoplanding")==0)
            {
                CMD_START_LANDING = 0;
            }
            char recchr[100];
            sprintf(recchr,"received data:%s\t CMD:%d\n",buff,CMD_START_LANDING);
            write(fd, recchr, strlen(recchr));
        }

        if(DJI_Z3_Pos.tar_detec_flag == 1 && CMD_START_LANDING == 1)
        {
            //fout_Yaw_Tar2Body<<currentAngle.yaw<<" "<<yawInDeg<<" "<<DJI_Z3_Pos.yaw*180.0/3.14159<<" "<<Yaw_Target2Body<<std::endl;
            cout<<currentAngle.yaw<<" "<<yawInDeg<<" "<<DJI_Z3_Pos.yaw*180.0/3.14159<<" "<<Yaw_Target2Body<<std::endl;

            //serial output
//            char recchr[512];
//            sprintf(recchr,"Yaw_tar2body:%f\n",Yaw_Target2Body);
//            write(fd, recchr, strlen(recchr));

            //0507 add vehicle control
            //velocityAndYawCtrl
//            uint ctrl_flag=(VERTICAL_VELOCITY | HORIZONTAL_VELOCITY | YAW_ANGLE |
//                            HORIZONTAL_BODY | STABLE_ENABLE);
//            uint ctrl_flag=(0x00 | 0x40 | 0x00 |
//                            0x02 | 0x00);
//            uint ctrl_flag=(VERTICAL_VELOCITY | HORIZONTAL_VELOCITY | YAW_RATE |
//                            HORIZONTAL_BODY | STABLE_ENABLE);
            uint ctrl_flag=(0x00 | 0x40 | 0x08 |
                            0x02 | 0x00);
            float32_t x = 0.0;
            float32_t y = 0.0;
            float32_t z = 0.0;
            float32_t yaw = 0.0;
            if(Yaw_Target2Body<10.0 && Yaw_Target2Body>-10.0)
            {
                if(DJI_Z3_Pos.Position_Y>500.0 || DJI_Z3_Pos.Position_Y<-500.0)
                    x = float32_t((DJI_Z3_Pos.Position_Y+0.1)>0.0? 1.0: -1.0);
                else
                    x = float32_t((DJI_Z3_Pos.Position_Y+0.1)*0.002);

                if(DJI_Z3_Pos.Position_X>500.0 || DJI_Z3_Pos.Position_X<-500.0)
                    y = float32_t(DJI_Z3_Pos.Position_X>0.0? -1.0: 1.0);
                else
                    y = float32_t(-DJI_Z3_Pos.Position_X*0.002);

                //yaw
                if(Yaw_Target2Body>0.0)
                    yaw = -2.0;
                else if(Yaw_Target2Body < -0.0)
                    yaw = 2.0;

                if(DJI_Z3_Pos.Position_Y<-0.3*DJI_Z3_Pos.Position_Z &&
                        DJI_Z3_Pos.Position_Y>0.3*DJI_Z3_Pos.Position_Z &&
                        DJI_Z3_Pos.Position_X<-0.3*DJI_Z3_Pos.Position_Z &&
                        DJI_Z3_Pos.Position_X>0.3*DJI_Z3_Pos.Position_Z)
                {
                    z = -0.5;
                    if(DJI_Z3_Pos.Position_Z > -200.0)
                    {
                        //monitoredLanding(vehicle);
                    }
                    char recchr[100];
                    sprintf(recchr,"Position_Z:%.2lf\n",DJI_Z3_Pos.Position_Z);
                    write(fd, recchr, strlen(recchr));
                }
            }
            else if(Yaw_Target2Body>10.0)
                yaw = -20.0;
            else if(Yaw_Target2Body < -10.0)
                yaw = 20.0;


            Control::CtrlData data(ctrl_flag, x, y, z, yaw);
            vehicle->control->flightCtrl(data);
            usleep(1000);
        }




        if(manifold_cam_exit())//return 1 on sucess, 0 if not ok
        {
            gimbalAngle.roll=0;
            gimbalAngle.pitch=0;
            gimbalAngle.yaw=0;
            gimbalAngle.mode = 1;
            gimbalAngle.duration = 10;
            vehicle->gimbal->setAngle(&gimbalAngle);
            sleep(2);

            //
            G_mission_statues = 1;
            pthread_join(gimbal_thread,NULL);

            //manifold serial close
            close(fd);

            break;
        }
    }
    pthread_join(read_thread, NULL);/*wait for read_thread exit*/
    while (!manifold_cam_exit())//return 1 on sucess, 0 if not ok
    {
        sleep(1);
    }

}
